/*
 * Copyright (C) 2021 HiHope Open Source Organization .
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 *
 * limitations under the License.
 */
#include <stdio.h>
#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "iot_watchdog.h"
#include "iot_pwm.h"
#include "hi_io.h"
#include "hi_adc.h"
#include "hi_time.h"
#include "icm.h"

#include <stdio.h>
#include <unistd.h>
#include "cmsis_os2.h"
#include "iot_i2c.h"
#include "iot_errno.h"

#include "ssd1306.h"
#include "ssd1306_tests.h"

unsigned char fifo_data[17];

#define     CLK_160M                160000000

uint16_t acc_data_X;
uint16_t acc_data_Y;
uint16_t acc_data_Z;

uint16_t acc_data_X0;
uint16_t acc_data_Y0;
uint16_t acc_data_Z0;

uint16_t acc_data_X1;
uint16_t acc_data_Y1;
uint16_t acc_data_Z1;

uint16_t gyro_data_X;
uint16_t gyro_data_Y;
uint16_t gyro_data_Z;

uint16_t gyro_data_X0;
uint16_t gyro_data_Y0;
uint16_t gyro_data_Z0;

uint16_t gyro_data_X1;
uint16_t gyro_data_Y1;
uint16_t gyro_data_Z1;

void *SixAxisTask(const char *arg)
{
    (void)arg;
    unsigned int IR_i;
    unsigned short data = 0; 
    char str[50];
    
    icm_initialize();


    uint32_t status = 0;
	uint8_t ReadFIFODataCmd [] = {FIFO_DATA_REG};

	status = IoTI2cWrite(WIFI_IOT_I2C_IDX_0, device_address, ReadFIFODataCmd, sizeof(ReadFIFODataCmd));
	osDelay(10);

	if (status != IOT_SUCCESS) {
            
		printf("===== Error: ICM40607 ReadFIFOData I2C write status = 0x%x! =====\r\n", status);

        snprintf(str, sizeof(str), "6 axis ERROR!");

		ssd1306_SetCursor(0,30);
        ssd1306_DrawString(str, Font_7x10, White);
	}else{
        snprintf(str, sizeof(str), "6 axis OK!");

		ssd1306_SetCursor(0,30);
        ssd1306_DrawString(str, Font_7x10, White);
    }
	

	IoTI2cRead(WIFI_IOT_I2C_IDX_0, device_address, fifo_data, sizeof(fifo_data));

	// printf("fifo_data[0]:%d\r\n",fifo_data[0]);		
    acc_data_X1 = fifo_data[1];
    acc_data_X0 = fifo_data[2];
    acc_data_X = (acc_data_X1<<8) | acc_data_X0;

    acc_data_Y1 = fifo_data[3];
    acc_data_Y0 = fifo_data[4];
    acc_data_Y = (acc_data_Y1<<8) | acc_data_Y0;

    acc_data_Z1 = fifo_data[5];
    acc_data_Z0 = fifo_data[6];
    acc_data_Z = (acc_data_Z1<<8) | acc_data_Z0;

    gyro_data_X1 = fifo_data[7];
    gyro_data_X0 = fifo_data[8];
    gyro_data_X = (gyro_data_X1<<8) | gyro_data_X0;

    gyro_data_Y1 = fifo_data[9];
    gyro_data_Y0 = fifo_data[10];
    gyro_data_Y = (gyro_data_Y1<<8) | gyro_data_Y0;

    gyro_data_Z1 = fifo_data[11];
    gyro_data_Z0 = fifo_data[12];
    gyro_data_Z = (gyro_data_Z1<<8) | gyro_data_Z0;

    usleep(100000); 

    printf("acc_data_X = %d\r\n", (unsigned int)acc_data_X);
    printf("acc_data_Y = %d\r\n", (unsigned int)acc_data_Y);
    printf("acc_data_Z = %d\r\n", (unsigned int)acc_data_Z);
    
    printf("gyro_data_X = %d\r\n", (unsigned int)gyro_data_X);
    printf("gyro_data_Y = %d\r\n", (unsigned int)gyro_data_Y);
    printf("gyro_data_Z = %d\r\n", (unsigned int)gyro_data_Z);


}
 
